﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Windows;
using System.Windows.Controls;
using System.Windows.Data;
using System.Windows.Documents;
using System.Windows.Input;
using System.Windows.Media;
using System.Windows.Media.Imaging;
using System.Windows.Navigation;
using System.Windows.Shapes;
using Microsoft.Kinect;
using Coding4Fun.Kinect.Wpf;

//IO: includes
using System.IO.Ports;
using System.Threading;

namespace SkeletalTracking
{

    // Interaction logic for MainWindow.xaml
    // Different variables are constructed which will be used later within the code.

    public partial class MainWindow : Window
    {
        static bool closing = false;
        const int skeletonCount = 6; 
        Skeleton[] allSkeletons = new Skeleton[skeletonCount];

        //Buttons
        List<Button> buttons;
        static Button selected;

        //Creating mutex so that aCommand at different loops are sent at the same time.
        private static Mutex mut = new Mutex();

        //Speed command variables
        static int rightSpeed;
        static int leftSpeed;
        static String command;
        static String aCommand;     

        //Serial communication
        static String message;
        private static SerialPort comport; 
        static bool sending = true;
        Thread receiveThread = new Thread(SerialComm);      //Placing the SerialComm port in a seperate thread for multithreaded running.

        //MainWindow, which is the constructor function, initialize the COM port with the specific baud rate.
        //It also adds the required buttons to the existing buttons list.
        //It finally creates a center point.
        public MainWindow()
        {
            //Initialize COM port
            comport = new SerialPort("COM3", 9600);
            comport.Open();

            //Initialize receive thread
            receiveThread.Start();

            InitializeComponent();
            InitializeButtons();

            //Initialize hover buttons
            kinectButton.Click += new RoutedEventHandler(kinectButton_Click);
            kinectButton.TimeInterval = 500;

            //Creating center point
            ColorImagePoint centerP = new ColorImagePoint();
            centerP.X = 320;
            centerP.Y = 240;
            CameraPosition(centerPoint, centerP);

            //Creating edges with the center rectangle
            ColorImagePoint centerR = new ColorImagePoint();
            centerR.X = 320;
            centerR.Y = 240;
            CameraPosition(centerRectangle, centerR);
        }

        
        //InitializeButtons function adds the required buttons to the existing buttons list.
        private void InitializeButtons()
        {
            buttons = new List<Button> { button1, button2, loadButton, fireButton };
        }

        //The CameraPosition method gets the mid point of element 
        // instead of in top/left corner
        private void CameraPosition(FrameworkElement element, ColorImagePoint point)
        {

            Canvas.SetLeft(element, point.X - element.Width / 2);
            Canvas.SetTop(element, point.Y - element.Height / 2);

        }

        //isHandOver is a boolean function returning true if right hand is pressing a button.
        //Otherwise it returns false.
        private bool isHandOver(FrameworkElement hand, List<Button> buttonslist)
        {
            var handTopLeft = new Point(Canvas.GetLeft(hand), Canvas.GetTop(hand));
            var handX = handTopLeft.X + hand.ActualWidth / 2;
            var handY = handTopLeft.Y + hand.ActualHeight / 2;

            foreach (Button target in buttonslist)
            {
                Point targetTopLeft = new Point(Canvas.GetLeft(target), Canvas.GetTop(target));
                if (handX > targetTopLeft.X &&
                    handX < targetTopLeft.X + target.Width &&
                    handY > targetTopLeft.Y &&
                    handY < targetTopLeft.Y + target.Height)
                {
                    selected = target;
                    return true;
                }
            }
            return false;
        }

        //The SendDirection method construct the appropriate string of command
        //based on the geometric position of left hand.
        private void SendDirection(ColorImagePoint control)
        {
            double conversionFactorX = 300 / kinectColorViewer1.Width;  // Scale factor for position coordinates
            double conversionFactorY = 300 / kinectColorViewer1.Height; // 1/2 the width and height of screen will be used

            double scaledX = control.X * conversionFactorX - 150; // Scale position and center
            scaledX = scaledX <= 100 ? (scaledX >= -100 ? scaledX : -100) : 100;    // Conditional statement.

            double scaledY = control.Y * conversionFactorY - 150;
            scaledY = scaledY <= 100 ? (scaledY >= -100 ? scaledY : -100) : 100;    // Conditional statement.

            int intX = Convert.ToInt16(scaledX);    // Convert to integer to conform to command format
            int intY = Convert.ToInt16(scaledY);

            // Channel mixing code
            rightSpeed = intY - intX;  // intY is actually opposite of what we want
            rightSpeed = rightSpeed <= 100 ? (rightSpeed >= -100 ? (Math.Abs(rightSpeed) >= 10 ? rightSpeed : 0) : -100) : 100; // Conditional statement.
            double dRightSpeed;
            dRightSpeed = rightSpeed >= 0 ? Math.Pow(rightSpeed,2) / 100 : -(Math.Pow(rightSpeed,2))/ 100;    // Quadratic normalizing.

            leftSpeed = intY + intX;
            leftSpeed = leftSpeed <= 100 ? (leftSpeed >= -100 ? (Math.Abs(leftSpeed) >= 10 ? leftSpeed : 0) : -100) : 100; //C CODE OF DOOM!!!!!!
            double dLeftSpeed;
            dLeftSpeed = leftSpeed >= 0 ? Math.Pow(leftSpeed, 2) / 100 : -(Math.Pow(leftSpeed, 2)) / 100;    //Quadratic normalizing.

            leftSpeed = Convert.ToInt16(dLeftSpeed);    
            rightSpeed = Convert.ToInt16(dRightSpeed);

            ScaledPosition.Content = String.Concat("Left: ", leftSpeed, "  Right: ", rightSpeed);

            // Serial port communication
            command = "w " + leftSpeed + " " + rightSpeed + "\n";   //giving the command string the appropriate values.
            Command.Content = command;
        }

        //SerialComm function sends the string of command to the Arduino program.
        private static void SerialComm()
        {
            while (!closing)
            {
                if (command != null && sending) 
                    try {
                        //Console.Out.Write("W_SEND: " + command);
                        comport.Write(command);
                        message = comport.ReadLine();        //Blocking function
                       // Console.Out.Write("W_RECEIVE: " + message);
                        }
                    catch (TimeoutException) { }
                    catch (InvalidOperationException) { }

                ////////////////////////
                if(aCommand != null && sending)
                try {

                    Console.Out.WriteLine("Send waiting for mutex.");
                    mut.WaitOne();                            // Wait until it is safe to enter.

                    Console.Out.Write("A_SEND: " + aCommand);     //Good for debugging.
                    comport.Write(aCommand);                    //Actual sending to com port.
                    aCommand=null;
                    message = comport.ReadLine();        //Blocking function
                    Console.Out.Write("A_RECEIVE: " + message);

                    mut.ReleaseMutex();                         // Release the Mutex.
                    Console.Out.WriteLine("Send released mutex.");
                }
                catch (TimeoutException) { }
                catch (InvalidOperationException) { }
                //////////////////////

                if (comport.BytesToRead != 0)
                {
                    message = comport.ReadLine();
                    Console.Out.Write("RECEIVE: " + message);
                }
                    
            }
            Console.Out.WriteLine("SerialComm exiting.");       //Closing the serial communication.
        }

        //button1_Click is the function which stops the program when clicked.
        private void button1_Click(object sender, RoutedEventArgs e)
        {
            if (sending)
            {
                sending = false;
                buttonPress.Content = "STOPPED";
            }
            else
            {
                sending = true;
                buttonPress.Content = "SENDING";
            }
        }

        //button2_Click just prints a massage for now but will be used later
        private void button2_Click(object sender, RoutedEventArgs e)
        {
            buttonPress.Content = "Button 2 clicked!";
        }

        private void loadButton_Click(object sender, RoutedEventArgs e)
        {
            Console.Out.WriteLine("Load waiting for mutex.");
            mut.WaitOne();                            // Wait until it is safe to enter.
            buttonPress.Content = "Loading ball!";
            aCommand = "a 0 1\n";                        //Turning conveyr but not motor.
            mut.ReleaseMutex();                         // Release the Mutex.
            Console.Out.WriteLine("Load released mutex.");
            
        }

        private void fireButton_Click(object sender, RoutedEventArgs e)
        {
            Console.Out.WriteLine("Fire waiting for mutex.");
            mut.WaitOne();                            // Wait until it is safe to enter.
            buttonPress.Content = "Firing ball!";
            aCommand = "a 1 1\n";                        //Turning both conveyr and motor on.
            mut.ReleaseMutex();                         // Release the Mutex.
            Console.Out.WriteLine("Fire released mutex.");
        }

        private void Window_Loaded(object sender, RoutedEventArgs e)
        {
            kinectSensorChooser1.KinectSensorChanged += new DependencyPropertyChangedEventHandler(kinectSensorChooser1_KinectSensorChanged);

        }

        void kinectSensorChooser1_KinectSensorChanged(object sender, DependencyPropertyChangedEventArgs e)
        {
            KinectSensor old = (KinectSensor)e.OldValue;

            StopKinect(old);

            KinectSensor sensor = (KinectSensor)e.NewValue;

            if (sensor == null)
            {
                return;
            }

            //Medium smoothing
            /*var parameters = new TransformSmoothParameters
            {
                Smoothing = 0.5f,
                Correction = 0.1f,
                Prediction = 0.5f,
                JitterRadius = 0.1f,
                MaxDeviationRadius = .1f
            };*/

            //High smoothing
            var parameters = new TransformSmoothParameters
            {
                Smoothing = 0.7f,
                Correction = 0.3f,
                Prediction = 1.0f,
                JitterRadius = 1.0f,
                MaxDeviationRadius = 1.0f
            };

            sensor.SkeletonStream.Enable(parameters);

            sensor.AllFramesReady += new EventHandler<AllFramesReadyEventArgs>(sensor_AllFramesReady);
            sensor.DepthStream.Enable(DepthImageFormat.Resolution640x480Fps30); 
            sensor.ColorStream.Enable(ColorImageFormat.RgbResolution640x480Fps30);

            try
            {
                sensor.Start();
            }
            catch (System.IO.IOException)
            {
                kinectSensorChooser1.AppConflictOccurred();
            }
        }

        void sensor_AllFramesReady(object sender, AllFramesReadyEventArgs e)
        {
            if (closing)
            {
                return;
            }

            //Get a skeleton
            Skeleton first =  GetFirstSkeleton(e);

            if (first == null)
            {
                return; 
            }



            //set scaled position
            //ScalePosition(headImage, first.Joints[JointType.Head]);
            //ScalePosition(leftEllipse, first.Joints[JointType.HandLeft]);
            //ScalePosition(kinectButton, first.Joints[JointType.HandRight]);

            GetCameraPoint(first, e);

            //Detect hand on button
            if (isHandOver(kinectButton, buttons)) kinectButton.Hovering();
            else kinectButton.Release();

        }

        void GetCameraPoint(Skeleton first, AllFramesReadyEventArgs e)
        {

            using (DepthImageFrame depth = e.OpenDepthImageFrame())
            {
                if (depth == null ||
                    kinectSensorChooser1.Kinect == null)
                {
                    return;
                }
                

                //Map a joint location to a point on the depth map
                //head
                DepthImagePoint headDepthPoint =
                    depth.MapFromSkeletonPoint(first.Joints[JointType.Head].Position);
                //left hand
                // Changed to left wrist 4/21/13
                DepthImagePoint leftDepthPoint =
                    depth.MapFromSkeletonPoint(first.Joints[JointType.WristLeft].Position);
                //right hand
                // Changed to right wrist 4/21/13
                DepthImagePoint rightDepthPoint =
                    depth.MapFromSkeletonPoint(first.Joints[JointType.WristRight].Position);


                //Map a depth point to a point on the color image
                //head
                ColorImagePoint headColorPoint =
                    depth.MapToColorImagePoint(headDepthPoint.X, headDepthPoint.Y,
                    ColorImageFormat.RgbResolution640x480Fps30);
                //left hand
                ColorImagePoint leftColorPoint =
                    depth.MapToColorImagePoint(leftDepthPoint.X, leftDepthPoint.Y,
                    ColorImageFormat.RgbResolution640x480Fps30);
                //right hand
                ColorImagePoint rightColorPoint =
                    depth.MapToColorImagePoint(rightDepthPoint.X, rightDepthPoint.Y,
                    ColorImageFormat.RgbResolution640x480Fps30);


                //Set location
                CameraPosition(headImage, headColorPoint);
                CameraPosition(leftEllipse, leftColorPoint);
                //Update position label
                LeftHandPosition.Content = String.Concat("Left hand X: ", leftColorPoint.X, "  Y: ", leftColorPoint.Y);

                CameraPosition(kinectButton, rightColorPoint);
                RightHandPosition.Content = String.Concat("Right hand X: ", rightColorPoint.X, "  Y: ", rightColorPoint.Y);

                //Send robot control commands
                SendDirection(leftColorPoint);

                
            }        
        }

        Skeleton GetFirstSkeleton(AllFramesReadyEventArgs e)
        {
            using (SkeletonFrame skeletonFrameData = e.OpenSkeletonFrame())
            {
                if (skeletonFrameData == null)
                {
                    return null; 
                }

                
                skeletonFrameData.CopySkeletonDataTo(allSkeletons);

                //get the first tracked skeleton
                Skeleton first = (from s in allSkeletons
                                         where s.TrackingState == SkeletonTrackingState.Tracked
                                         select s).FirstOrDefault();

                return first;

            }
        }

        private void StopKinect(KinectSensor sensor)
        {
            if (sensor != null)
            {
                if (sensor.IsRunning)
                {
                    //stop sensor 
                    sensor.Stop();

                    //stop audio if not null
                    if (sensor.AudioSource != null)
                    {
                        sensor.AudioSource.Stop();
                    }


                }
            }
        }

        private void ScalePosition(FrameworkElement element, Joint joint)
        {
            //convert the value to X/Y
            //Joint scaledJoint = joint.ScaleTo(1280, 720); 
            
            //convert & scale (.3 = means 1/3 of joint distance)
            Joint scaledJoint = joint.ScaleTo(1280, 720, .3f, .3f);

            Canvas.SetLeft(element, scaledJoint.Position.X);
            Canvas.SetTop(element, scaledJoint.Position.Y); 
            
        }

        private void Window_Closing(object sender, System.ComponentModel.CancelEventArgs e)
        {
            closing = true;
            Console.Out.WriteLine("Stopping kinect");
            StopKinect(kinectSensorChooser1.Kinect);
            Console.Out.WriteLine("Join SerialComm thread");
            receiveThread.Join();
            Console.Out.WriteLine("Comport closing");
            comport.Close();
            Console.Out.WriteLine("Window closing");
        }

        void kinectButton_Click(object sender, RoutedEventArgs e)
        {
            selected.RaiseEvent(new RoutedEventArgs(Button.ClickEvent, selected));
        }

        private void kinectButton_Loaded(object sender, RoutedEventArgs e)
        {
            kinectButton.ImageSource = "/Images/RightHand.png";
            kinectButton.ActiveImageSource = "/Images/RightHand.png";
        }

        private void kinectColorViewer1_Loaded(object sender, RoutedEventArgs e)
        {

        }
    }
}
